{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true,
    "pycharm": {
     "is_executing": false
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "text": [
      "Processing 1 of 3953\n"
     ],
     "output_type": "stream"
    },
    {
     "name": "stderr",
     "text": [
      "c:\\users\\s-vec\\appdata\\local\\programs\\python\\python37\\lib\\site-packages\\matplotlib\\collections.py:874: RuntimeWarning: invalid value encountered in sqrt\n  scale = np.sqrt(self._sizes) * dpi / 72.0 * self._factor\n"
     ],
     "output_type": "stream"
    },
    {
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mFileNotFoundError\u001b[0m                         Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-1-5aaa0e3d5948>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m\u001b[0m\n\u001b[0;32m     44\u001b[0m     \u001b[0max\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mset_ylabel\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;34m'Y'\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     45\u001b[0m     \u001b[0max\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mset_zlabel\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;34m'Z'\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m---> 46\u001b[1;33m     \u001b[0mplt\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0msavefig\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mos\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mpath\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mjoin\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mradarImg_path\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mstr\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mtimestamp\u001b[0m\u001b[1;33m)\u001b[0m \u001b[1;33m+\u001b[0m \u001b[1;34m'.jpg'\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m     47\u001b[0m     \u001b[0mplt\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mclf\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     48\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mc:\\users\\s-vec\\appdata\\local\\programs\\python\\python37\\lib\\site-packages\\matplotlib\\pyplot.py\u001b[0m in \u001b[0;36msavefig\u001b[1;34m(*args, **kwargs)\u001b[0m\n\u001b[0;32m    687\u001b[0m \u001b[1;32mdef\u001b[0m \u001b[0msavefig\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m*\u001b[0m\u001b[0margs\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;33m**\u001b[0m\u001b[0mkwargs\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    688\u001b[0m     \u001b[0mfig\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mgcf\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m--> 689\u001b[1;33m     \u001b[0mres\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mfig\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0msavefig\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m*\u001b[0m\u001b[0margs\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;33m**\u001b[0m\u001b[0mkwargs\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m    690\u001b[0m     \u001b[0mfig\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mcanvas\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mdraw_idle\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m)\u001b[0m   \u001b[1;31m# need this if 'transparent=True' to reset colors\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    691\u001b[0m     \u001b[1;32mreturn\u001b[0m \u001b[0mres\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mc:\\users\\s-vec\\appdata\\local\\programs\\python\\python37\\lib\\site-packages\\matplotlib\\figure.py\u001b[0m in \u001b[0;36msavefig\u001b[1;34m(self, fname, frameon, transparent, **kwargs)\u001b[0m\n\u001b[0;32m   2092\u001b[0m             \u001b[0mself\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mset_frameon\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mframeon\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   2093\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m-> 2094\u001b[1;33m         \u001b[0mself\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mcanvas\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mprint_figure\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mfname\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;33m**\u001b[0m\u001b[0mkwargs\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m   2095\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   2096\u001b[0m         \u001b[1;32mif\u001b[0m \u001b[0mframeon\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mc:\\users\\s-vec\\appdata\\local\\programs\\python\\python37\\lib\\site-packages\\matplotlib\\backend_bases.py\u001b[0m in \u001b[0;36mprint_figure\u001b[1;34m(self, filename, dpi, facecolor, edgecolor, orientation, format, bbox_inches, **kwargs)\u001b[0m\n\u001b[0;32m   2073\u001b[0m                     \u001b[0morientation\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0morientation\u001b[0m\u001b[1;33m,\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   2074\u001b[0m                     \u001b[0mbbox_inches_restore\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0m_bbox_inches_restore\u001b[0m\u001b[1;33m,\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m-> 2075\u001b[1;33m                     **kwargs)\n\u001b[0m\u001b[0;32m   2076\u001b[0m             \u001b[1;32mfinally\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   2077\u001b[0m                 \u001b[1;32mif\u001b[0m \u001b[0mbbox_inches\u001b[0m \u001b[1;32mand\u001b[0m \u001b[0mrestore_bbox\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mc:\\users\\s-vec\\appdata\\local\\programs\\python\\python37\\lib\\site-packages\\matplotlib\\backends\\backend_agg.py\u001b[0m in \u001b[0;36mprint_jpg\u001b[1;34m(self, filename_or_obj, dryrun, *args, **kwargs)\u001b[0m\n\u001b[0;32m    576\u001b[0m                 \u001b[0moptions\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;34m'dpi'\u001b[0m\u001b[1;33m]\u001b[0m \u001b[1;33m=\u001b[0m \u001b[1;33m(\u001b[0m\u001b[0moptions\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;34m'dpi'\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0moptions\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;34m'dpi'\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    577\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m--> 578\u001b[1;33m             \u001b[1;32mreturn\u001b[0m \u001b[0mbackground\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0msave\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mfilename_or_obj\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mformat\u001b[0m\u001b[1;33m=\u001b[0m\u001b[1;34m'jpeg'\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;33m**\u001b[0m\u001b[0moptions\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m    579\u001b[0m         \u001b[0mprint_jpeg\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mprint_jpg\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    580\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mc:\\users\\s-vec\\appdata\\local\\programs\\python\\python37\\lib\\site-packages\\PIL\\Image.py\u001b[0m in \u001b[0;36msave\u001b[1;34m(self, fp, format, **params)\u001b[0m\n\u001b[0;32m   2002\u001b[0m                 \u001b[1;31m# Open also for reading (\"+\"), because TIFF save_all\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   2003\u001b[0m                 \u001b[1;31m# writer needs to go back and edit the written data.\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m-> 2004\u001b[1;33m                 \u001b[0mfp\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mbuiltins\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mopen\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mfilename\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;34m\"w+b\"\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m   2005\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   2006\u001b[0m         \u001b[1;32mtry\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mFileNotFoundError\u001b[0m: [Errno 2] No such file or directory: 'radar_test\\\\1564366262.1876683.jpg'"
     ],
     "ename": "FileNotFoundError",
     "evalue": "[Errno 2] No such file or directory: 'radar_test\\\\1564366262.1876683.jpg'",
     "output_type": "error"
    }
   ],
   "source": [
    "import numpy as np\n",
    "import pickle\n",
    "import os\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "from mpl_toolkits.mplot3d import Axes3D\n",
    "from matplotlib import style\n",
    "\n",
    "from PIL import Image, ImageDraw\n",
    "\n",
    "radarData_path =  'data/072819_zl_onNotOn/f_data-2019-07-28_22-11-01.258054_zl_onNotOn_rnn/f_data.p'\n",
    "videoData_path = 'data/072819_zl_onNotOn/v_data-2019-07-28_22-10-32.249041_zl_onNotOn_rnn/cam1'\n",
    "mergedImg_path = 'E:/figures/zl_onNotOn'\n",
    "radarImg_path = 'radar_test'\n",
    "\n",
    "radar_data = list(pickle.load(open(radarData_path, 'rb')).items())\n",
    "radar_data.sort(key=lambda x: x[0])  # sort by timestamp\n",
    "videoData_list = os.listdir(videoData_path)\n",
    "videoData_timestamps = list(map(lambda x: float(x.strip('.jpg')), videoData_list))\n",
    "\n",
    "style.use('fivethirtyeight')\n",
    "color = 'rgb(255, 255, 255)'\n",
    "for timestamp, data in radar_data:\n",
    "    i = radar_data.index((timestamp, data))\n",
    "    print('Processing ' + str(i + 1) + ' of ' + str(len(radar_data)))\n",
    "\n",
    "    closest_video_timestamp = min(videoData_timestamps,\n",
    "                                  key=lambda x: abs(x - timestamp))\n",
    "    closest_video_path = os.path.join(videoData_path, str(closest_video_timestamp) + '.jpg')\n",
    "    closest_video_img = Image.open(closest_video_path)\n",
    "\n",
    "    # plot the radar scatter\n",
    "    fig = plt.figure()\n",
    "    ax = fig.add_subplot(111, projection='3d')\n",
    "    ax.set_xlim((-0.3, 0.3))\n",
    "    ax.set_ylim((-0.3, 0.3))\n",
    "    ax.set_zlim((-0.3, 0.3))\n",
    "\n",
    "    # ax.scatter(data['x'], data['y'], data['z'], c=data['doppler'], marker='D')\n",
    "\n",
    "    ax.scatter(data['x'], data['y'], data['z'], s=10 * data['doppler'], marker='D')\n",
    "\n",
    "    ax.set_xlabel('X')\n",
    "    ax.set_ylabel('Y')\n",
    "    ax.set_zlabel('Z')\n",
    "    plt.savefig(os.path.join(radarImg_path, str(timestamp) + '.jpg'))\n",
    "    plt.clf()\n",
    "\n",
    "    radar_img = Image.open(os.path.join(radarImg_path, str(timestamp) + '.jpg'))\n",
    "\n",
    "    images = [closest_video_img, radar_img]\n",
    "\n",
    "    widths, heights = zip(*(i.size for i in images))\n",
    "\n",
    "    total_width = sum(widths)\n",
    "    max_height = max(heights)\n",
    "\n",
    "    new_im = Image.new('RGB', (total_width, max_height))\n",
    "\n",
    "    x_offset = 0\n",
    "    for im in images:\n",
    "        new_im.paste(im, (x_offset, 0))\n",
    "        x_offset += im.size[0]\n",
    "\n",
    "    timestamp_difference = abs(float(timestamp) - float(closest_video_timestamp))\n",
    "    draw = ImageDraw.Draw(new_im)\n",
    "\n",
    "    # draw the timestamp difference on the image\n",
    "    (x, y) = (50, 70)\n",
    "    message = \"Timestamp Difference, abs(rt-vt): \" + str(timestamp_difference)\n",
    "    draw.text((x,y), message, fill=color)\n",
    "\n",
    "    # draw the timestamp\n",
    "    (x, y) = (50, 50)\n",
    "    message = \"Timestamp: \" + str(timestamp)\n",
    "    draw.text((x, y), message, fill=color)\n",
    "\n",
    "    new_im.save(os.path.join(mergedImg_path, str(timestamp) + '.jpg'))\n"
   ]
  }
 ],
 "metadata": {
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.6"
  },
  "kernelspec": {
   "name": "python3",
   "language": "python",
   "display_name": "Python 3"
  },
  "pycharm": {
   "stem_cell": {
    "cell_type": "raw",
    "source": [],
    "metadata": {
     "collapsed": false
    }
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}